global l bound result npout
l=[1,1];
bound=[ -pi     pi;
        -pi     pi;
        -pi/4   pi/4;
        -pi/4   pi/4;
        0       6;
        0       6];
npout=40;
% hh=findobj(gcf,'tag','x1');
% xs=str2num(get(hh,'string'));
% hh=findobj(gcf,'tag','y1');
% ys=str2num(get(hh,'string'));
% hh=findobj(gcf,'tag','x2');
% xg=str2num(get(hh,'string'));
% hh=findobj(gcf,'tag','y2');
% yg=str2num(get(hh,'string'));
xs=1; ys=0;
xg=0.6; yg=1.4;
qs=invkin(xs,ys,l);
qg=invkin(xg,yg,l);
para=[qs,qg];

% cacu_result(2,para,100,120,@fitnessf);
% [x1,x2,x4]=trajectory_planningf_GA(para,100,100,@fitnessf,@trajplot2,@paraplot2)
% [x1,x2,x4]=trajectory_planningf_PSO(para,100,200,@fitnessf,@trajplot2,@paraplot2)
[x1,x2,x4]=trajectory_planningf_BSO(para,100,50,@fitnessf,@trajplot2,@paraplot2)
% [x21,x22,x23,x24]=trajectory_planningf_xBSO(para,@recor2,@trajplot2,@paraplot2)

function aa = fitnessf(pop,para)
global l npout
[pops,numvar]=size(pop);
for i=1:pops
    a_v=[ 2;  %ft 2
          1;  %fq 1.8
          0;  %fdis 2
          0]; %time 1
    bsol=pop(i,:);
    time=bsol(5)+bsol(6);
    kk=trajt(para,bsol,npout);
%     tt=torque(kk);
%     ft=ftorque(tt);
    ft=ftorque(kk(5:6,:));
    ffq=kk(1:2,:);
    fq=sum(sum(abs(diff(ffq'))));
    pos=forkin(ffq,l);
    xx=pos(1,:);yy=pos(2,:);
    x=diff(xx);
    y=diff(yy);
    fdis=sum(sqrt(x.^2+y.^2));
    fit_v=[ft,fq,fdis,time];
    fit=fit_v*a_v;
    aa(i)=1/fit;
end
end

function trajplot2(para,bsol,fit,i,n)
global l kk npout
    qs=para(1:2);
    qm=bsol(1:2);
    qg=para(3:4);
    
    [xt,yt]=angls2links([qs' qm' qg'],l);
    kk=trajt(para,bsol,npout);
    pq=kk(1:2,:);
    pcart=forkin(pq,l);
    px=pcart(1,:);py=pcart(2,:);
    plot(px,py)
    hold on
    axis([-2.5 2.5 -2.5 2.5])
    text(1.75,2.4,['gen. No.',num2str(i)]);
    text(1.75,2.1,['fit=',num2str(fit)]);
    plot(xt',yt')
    hold on
    plot(0,0,'ko')
    plot(xt(:,size(xt,2)/2),yt(:,size(xt,2)/2),'bo')
    hold off
end

function paraplot2(n,maxf,bsol,rec)
global l kk
%     trec=rec(1,:);
%     qrec=rec(2,:);
%     drec=rec(3,:);
%     torrec=rec(4,:);
figure,manimove_anime(kk(1:2,:),l);

e=[1:n];
figure,plot(e,1./maxf)
xlabel('iteration')
ylabel('min. fitness')
end